package edu.csusm.spotrobot;

import com.sun.spot.sensorboard.EDemoBoard;
import java.io.IOException;
import java.util.*;

/**
 *
 * Used to send command to the PIC to control the motors.
 * ~1 tic per cm
 *
 * @author Zebulon Evans
 */
public class MotorController {
    // COMMANDS
    // int enums, enum not supported in this version
    
    /** 
     * Go forward without paying attention if we are actually going straight.
     * The second arguement needs to be a speed when used in sendCommand.
     */
    public static final int FORWARD         = 65;
    /** 
     * Rotate left until told otherwise.
     * The second arguement needs to be a speed when used in sendCommand.
     */
    public static final int ROTATE_LEFT     = 69;
    /** 
     * Rotate right until told otherwise.
     * The second arguement needs to be a speed when used in sendCommand.
     */
    public static final int ROTATE_RIGHT    = 70;
    /**
     * Rotate left a number of ticks.
     * The second arguement needs to be the number of ticks in sendCommand.
     */
    public static final int ROTATE_LEFT_N   = 75;
    /**
     * Rotate right a number of ticks.
     * The second arguement needs to be the number of ticks in sendCommand.
     */
    public static final int ROTATE_RIGHT_N  = 76;
    /**
     * Go backward a number of ticks.  May not go straight.
     * The second arguement needs to be the number of ticks in sendCommand.
     */
    public static final int REVERSE_N       = 79;
    /**
     * Go forward a number of ticks.  May not go straight.
     * The second arguement needs to be the number of ticks in sendCommand.
     */
    public static final int FORWARD_N       = 83;
    /**
     * Brake the motors.
     * The second arguement is ignored in sendCommand.
     */
    public static final int BRAKE           = 80;
    /**
     * Go forward a number of ticks.  Slower but will go stright.
     * The second arguement needs to be the number of ticks in sendCommand.
     */
    public static final int STRAIGHT        = 86;
    /** 
     * Set the ticks per second for the N (ticks) commands.
     */
    public static final int SET_TICK_SPEED  = 89;    
    
    private EDemoBoard demo = EDemoBoard.getInstance();
    
    public MotorController(){

    }
    
    /**
     * Sends a command to the PIC for to control the motors.  The second
     * parameter will be speed or the number of ticks to move depending on the
     * command.  Sends a command and tries to get acknowledgment of the command
     * from the PIC.
     *
     * @param int the command to send
     * @param int the speed or number of ticks depending on the command
     * @return boolean true if command was send, reciept was acknowledged, and
     *      command complete acknowledgement was received.
     */
    public boolean sendCommand( int command, int speed ){
        if( speed < 1 ){
            System.out.println("Received a bad speed in the motor controller returning");
            return true;
        }
        
        flushBuffer();
        int recCommand = 0;
        int recSpeed = 0;
        boolean commandAck = false;
        byte ackByte = 0;
        while( !commandAck ){
            demo.writeUART((byte)0x21);
            demo.writeUART((byte)command);
            flushBuffer();
            demo.writeUART((byte)speed);
            
            System.out.println("Sending: "+(byte)0x21+","+(byte)command+","+(byte)speed);
            try{
                Thread.sleep(100);
                ackByte = demo.readUART(10000);
                recCommand = demo.readUART(100);
                recSpeed = demo.readUART(100);
                System.out.println("Ack: "+(byte)ackByte+","+(byte)recCommand+","+(byte)recSpeed);
            }catch(Exception e){}
            
            if( ackByte == '"' )
                commandAck = true;
            else
                return false;           
        }
        
        // block until we recieve the command back.  Change this to have a timeout
        byte data[] = new byte[3];
        int counter = 0;
        try{
            Thread.sleep(100);
            System.out.print("Receiving: ");
            data[0] = (byte)demo.readUART(8000);
            data[1] = (byte)demo.readUART(100);
            data[2] = (byte)demo.readUART(100);
                    
            System.out.print((byte)data[0]+",");
            System.out.print((byte)data[1]+",");
            System.out.println((byte)data[3]);
            
        }catch(Exception e){}
        
        if( data[1] != recCommand || data[2] != recSpeed){
            System.out.print("I did not receive a command complete!");
            return false;
        }
        
        flushBuffer();

        System.out.println("Leaving the MotorController \r");
        return true;
    }
    
    /**
     * Flush the recieve buffer for the UART for serial communications.
     */
    private void flushBuffer(){
        //flush receive buffer.
        try {
            while(demo.availableUART() > 0)
                demo.readUART(100);
        } catch(IOException ioe) {}
    }
    
    /**
     * Go to sleep without having to catch the exception.  This is not a good
     * practice and should be handled at a higher level.
     */
    private void toSleep( int msec ){
        //TODO fix me catch exceptions at a higher level
        try {
            Thread.sleep(msec);
        } catch (InterruptedException e) {
            //We've been interrupted: exit the thread
            return;
        }
    }
    
}
